// OC1A - PB1 - 9
// OC1B - PB2 - 10
//
// ENA - PB1 - 9
// ENB - PB2 - 10
// IN1 - PD2 - 2
// IN2 - PD3 - 3
// IN3 - PD4 - 4
// IN4 - PD5 - 5

//Port C - R/C Receiver
// PC0 - F/B, PC1 - L/R
void rxSetup() {
  DDRC   &= 0b11000000; // Set Port-C[5:0] to input
  PORTC  |= 0b00111111; // Enable internal pull-up
  PCMSK1 |= 0b00111111; // Enable per-pin interupt PCINT[13:8]
  PCICR  |= 0b00000010; // Enable per-register interrupt PCIE1
}

volatile uint16_t rcValue[6] = {1502, 1502, 1502, 1502, 1502, 1502};
volatile uint32_t currentTime = 0;
//volatile int8_t speedA, speedB;

#define MOTOR_PIN_ENA 9
#define MOTOR_PIN_ENB 10
#define MOTOR_PIN_IN1 2
#define MOTOR_PIN_IN2 3
#define MOTOR_PIN_IN3 4
#define MOTOR_PIN_IN4 5

void setup() {
  rxSetup();
  pinMode(MOTOR_PIN_ENA, OUTPUT);
  pinMode(MOTOR_PIN_IN1, OUTPUT);
  pinMode(MOTOR_PIN_IN2, OUTPUT);
  pinMode(MOTOR_PIN_ENB, OUTPUT);
  pinMode(MOTOR_PIN_IN3, OUTPUT);
  pinMode(MOTOR_PIN_IN4, OUTPUT);
  digitalWrite(MOTOR_PIN_ENA, LOW);
  digitalWrite(MOTOR_PIN_ENB, LOW);//stop driving
  digitalWrite(MOTOR_PIN_IN1, LOW); 
  digitalWrite(MOTOR_PIN_IN2, HIGH);//setting motorA's directon
  digitalWrite(MOTOR_PIN_IN3, LOW);
  digitalWrite(MOTOR_PIN_IN4, HIGH);//setting motorB's directon
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) ; // phase and frequency correct mode. NON-inverted mode
  TCCR1B = _BV(WGM13) | _BV(CS10); //No Prescaling
  ICR1 = 800;
  OCR1A = 0;
  OCR1B = 0;
  Serial.begin(9600);
}

int val = 0;
int incr = 1;
boolean forward = false;

void updateMotors () {
  int16_t ch[2];
  int16_t speedA, speedB;
  for ( byte i = 0; i < 2; i++ ) {
    ch[i] = rcValue[i] - 1500;
    if ( -50 < ch[i] && ch[i] < 50 ) ch[i] = 0;
    if ( ch[i] < -450 ) ch[i] = -450;
    if ( ch[i] >  450 ) ch[i] =  450;
    ch[i] = (float)ch[i]*200.0/450.;
  }
  
  //String serial_string;
  //serial_string = String( "ch[0]: " + String(ch[0], DEC) + "; ch[1]: " + String(ch[1], DEC) );
  //Serial.println(serial_string);

  speedA = ch[0] + ch[1];
  speedB = ch[0] - ch[1];
  if ( speedA >  200 ) speedA =  200;
  if ( speedA < -200 ) speedA = -200;
  if ( speedB >  200 ) speedB =  200;
  if ( speedB < -200 ) speedB = -200;

  if ( speedA == 0 ) {
    OCR1A = 0;
  } else {
    if ( speedA > 0 ) {
      digitalWrite(MOTOR_PIN_IN1, LOW); 
      digitalWrite(MOTOR_PIN_IN2, HIGH);
      OCR1A = 600 + speedA;
    } else {
      digitalWrite(MOTOR_PIN_IN1, HIGH); 
      digitalWrite(MOTOR_PIN_IN2, LOW);
      OCR1A = 600 - speedA;
    }
  }

  if ( speedB == 0 ) {
    OCR1B = 0;
  } else {
    if ( speedB > 0 ) {
      digitalWrite(MOTOR_PIN_IN3, LOW); 
      digitalWrite(MOTOR_PIN_IN4, HIGH);
      OCR1B = 600 + speedB;
    } else {
      digitalWrite(MOTOR_PIN_IN3, HIGH); 
      digitalWrite(MOTOR_PIN_IN4, LOW);
      OCR1B = 600 - speedB;
    }
  }
}

void loop()
{
  static uint32_t c2hzTime = 0;
  static uint32_t c50hzTime = 0;
  String serial_string;
  sei();
  currentTime = micros();

  if ( currentTime > c50hzTime ) { // 50Hz
    c50hzTime = currentTime + 20000;
    updateMotors();
  }

  if ( currentTime > c2hzTime ) { // 2Hz
    c2hzTime = currentTime + 500000;
    serial_string = String( "Ch1: " + String(rcValue[0], DEC) + "; Ch2: " + String(rcValue[1], DEC) );
    Serial.println(serial_string);
    serial_string = String( "OCR1A: " + String(OCR1A, DEC) + "; OCR1B: " + String(OCR1B, DEC) );
    Serial.println(serial_string);
  }
}

ISR(PCINT1_vect) {
  uint8_t pin;
  uint8_t mask;
  static uint8_t pinLast;
  uint16_t cTime, dTime;
  static uint16_t edgeTime[6];
  
  pin = PINC; // Read pins status
  mask = pin ^ pinLast; // Create mask for changed pins
  pinLast = pin;
  cTime = micros();
  sei();
  for( uint8_t i = 0; i < 6; i++ ) {
    if ( mask & (1<<i) ) { // if pini changed
      if ( !( pin & (1<<i) ) ) { // Fall edge
        dTime = cTime - edgeTime[i];
        if ( 900 < dTime && dTime < 2200 )
          rcValue[i] = dTime;
          //if ( dTime < 1000 ) rcValue[i] = 1000;
          //if ( dTime > 2000 ) rcValue[i] = 2000;
      } else { // Rise edge
        edgeTime[i] = cTime;
      }
    }
  }
}
